#include "Matrix3.h"
#include <vector>


Matrix3::Matrix3()
{
    for( int i = 0; i < 9; i++)
        m_data[i] = 0.0f;
    m_data[0] = m_data[4] = m_data[8] = 1.0f;
}
Matrix3::Matrix3(const Vec3f& one, const Vec3f& two, const Vec3f& three)
{
    SetComponents(one,two,three);
}
Matrix3::Matrix3(float aX,float aY,float aZ,float bX,float bY,float bZ,float cX,float cY,float cZ)
{
    m_data[0] = aX;
    m_data[3] = bX;
    m_data[6] = cX;

    m_data[1] = aY;
    m_data[4] = bY;
    m_data[7] = cY;

    m_data[2] = aZ;
    m_data[5] = bZ;
    m_data[8] = cZ;
}
void Matrix3::SetComponents(const Vec3f& one, const Vec3f& two, const Vec3f& three)
{
    m_data[0] = one.GetX();
    m_data[3] = two.GetX();
    m_data[6] = three.GetX();

    m_data[1] = one.GetY();
    m_data[4] = two.GetY();
    m_data[7] = three.GetY();

    m_data[2] = one.GetZ();
    m_data[5] = two.GetZ();
    m_data[8] = three.GetZ();
}
void Matrix3::SetDiagonal(float a, float b, float c)
{
    SetInertiaTensorCoeffs(a, b, c);
}
void Matrix3::SetInertiaTensorCoeffs(float ix, float iy, float iz,
                                          float ixy, float ixz, float iyz)
{
    m_data[0] = ix;
    m_data[3] = m_data[1] = -ixy;
    m_data[6] = m_data[2] = -ixz;
    m_data[4] = iy;
    m_data[7] = m_data[5] = -iyz;
    m_data[8] = iz;
}
void Matrix3::SetBlockInertiaTensor(const Vec3f& vec, float mass)
{
    Vec3f square = vec.componentProduct(vec);
    SetInertiaTensorCoeffs(0.3f*mass*(square.GetY() + square.GetZ()),
                           0.3f*mass*(square.GetX() + square.GetZ()),
                           0.3f*mass*(square.GetX() + square.GetY()));
}

Vec3f Matrix3::operator*(const Vec3f& other) const
{
    return Vec3f(
        other.GetX() * m_data[0] + other.GetY() * m_data[3] + other.GetZ() * m_data[6],
        other.GetX() * m_data[1] + other.GetY() * m_data[4] + other.GetZ() * m_data[7],
        other.GetX() * m_data[2] + other.GetY() * m_data[5] + other.GetZ() * m_data[8]);
}

Matrix3& Matrix3::operator+=(const Matrix3& o)
{
    m_data[0] += o.m_data[0]; m_data[3] += o.m_data[3]; m_data[6] += o.m_data[6];
    m_data[1] += o.m_data[1]; m_data[4] += o.m_data[4]; m_data[7] += o.m_data[7];
    m_data[2] += o.m_data[2]; m_data[5] += o.m_data[5]; m_data[8] += o.m_data[8];
    return *this;
}
Vec3f Matrix3::Transform(const Vec3f& v) const
{
    return (*this) * v;
}
void Matrix3::SetTranspose(const Matrix3& m) 
{
    m_data[0] = m.m_data[0];
    m_data[3] = m.m_data[1];
    m_data[6] = m.m_data[2];
    m_data[1] = m.m_data[3];
    m_data[4] = m.m_data[4];
    m_data[7] = m.m_data[5];
    m_data[2] = m.m_data[6];
    m_data[5] = m.m_data[7];
    m_data[8] = m.m_data[8];
}
Matrix3 Matrix3::Transpose() const
{
    Matrix3 result;
    result.SetTranspose(*this);
    return result;
}

Vec3f Matrix3::TransformTranspose(const Vec3f& v) const
{
    return Vec3f(
        v.GetX() * m_data[0] + v.GetY() * m_data[1] + v.GetZ() * m_data[2],
        v.GetX() * m_data[3] + v.GetY() * m_data[4] + v.GetZ() * m_data[5],
        v.GetX() * m_data[6] + v.GetY() * m_data[7] + v.GetZ() * m_data[8]);
}

Vec3f Matrix3::GetRow(int index) const
{
    if(index == 1)
        return Vec3f(m_data[1], m_data[4], m_data[7]);
    else if(index == 2)
        return Vec3f(m_data[2], m_data[5], m_data[8]);
    else
        return Vec3f(m_data[0], m_data[3], m_data[6]);
}
Matrix3 Matrix3::operator*(const Matrix3& o) const
{
    return Matrix3(
                m_data[0]*o.m_data[0] + m_data[3]*o.m_data[1] + m_data[6]*o.m_data[2],
                m_data[0]*o.m_data[3] + m_data[3]*o.m_data[4] + m_data[6]*o.m_data[5],
                m_data[0]*o.m_data[6] + m_data[3]*o.m_data[7] + m_data[6]*o.m_data[8],

                m_data[1]*o.m_data[0] + m_data[4]*o.m_data[1] + m_data[7]*o.m_data[2],
                m_data[1]*o.m_data[3] + m_data[4]*o.m_data[4] + m_data[7]*o.m_data[5],
                m_data[1]*o.m_data[6] + m_data[4]*o.m_data[7] + m_data[7]*o.m_data[8],

                m_data[2]*o.m_data[0] + m_data[5]*o.m_data[1] + m_data[8]*o.m_data[2],
                m_data[2]*o.m_data[3] + m_data[5]*o.m_data[4] + m_data[8]*o.m_data[5],
                m_data[2]*o.m_data[6] + m_data[5]*o.m_data[7] + m_data[8]*o.m_data[8]
                );
}

Matrix3& Matrix3::operator*=(const Matrix3& o)
{
    float t1 = 0.0f;
    float t2 = 0.0f;
    float t3 = 0.0f;

    t1 = m_data[0]*o.m_data[0] + m_data[3]*o.m_data[1] + m_data[6]*o.m_data[2];
    t2 = m_data[0]*o.m_data[3] + m_data[3]*o.m_data[4] + m_data[6]*o.m_data[5];
    t3 = m_data[0]*o.m_data[6] + m_data[3]*o.m_data[7] + m_data[6]*o.m_data[8];
    m_data[0] = t1;
    m_data[3] = t2;
    m_data[6] = t3;

    t1 = m_data[1]*o.m_data[0] + m_data[4]*o.m_data[1] + m_data[7]*o.m_data[2];
    t2 = m_data[1]*o.m_data[3] + m_data[4]*o.m_data[4] + m_data[7]*o.m_data[5];
    t3 = m_data[1]*o.m_data[6] + m_data[4]*o.m_data[7] + m_data[7]*o.m_data[8];
    m_data[1] = t1;
    m_data[4] = t2;
    m_data[7] = t3;

    t1 = m_data[2]*o.m_data[0] + m_data[5]*o.m_data[1] + m_data[8]*o.m_data[2];
    t2 = m_data[2]*o.m_data[3] + m_data[5]*o.m_data[4] + m_data[8]*o.m_data[5];
    t3 = m_data[2]*o.m_data[6] + m_data[5]*o.m_data[7] + m_data[8]*o.m_data[8];
    m_data[2] = t1;
    m_data[5] = t2;
    m_data[8] = t3;
    return *this;
}

Matrix3& Matrix3::operator*=(const float scalar)
{
    m_data[0] *= scalar; m_data[1] *= scalar; m_data[2] *= scalar;
    m_data[3] *= scalar; m_data[4] *= scalar; m_data[5] *= scalar;
    m_data[6] *= scalar; m_data[7] *= scalar; m_data[8] *= scalar;
    return *this;
}

void Matrix3::SetOrientation(const Quaternion& q)
{
    m_data[0] = 1 - (2 *(q.m_j * q.m_j) + 2 *(q.m_k * q.m_k));
    m_data[3] =     (2 *(q.m_i * q.m_j) + 2 *(q.m_k * q.m_r));
    m_data[6] =     (2 *(q.m_i * q.m_k) - 2 *(q.m_j * q.m_r));
    m_data[1] =     (2 *(q.m_i * q.m_j) - 2 *(q.m_k * q.m_r));
    m_data[4] = 1 - (2 *(q.m_i * q.m_i) + 2 *(q.m_k * q.m_k));
    m_data[7] =     (2 *(q.m_j * q.m_k) + 2 *(q.m_i * q.m_r));
    m_data[2] =     (2 *(q.m_i * q.m_k) + 2 *(q.m_j * q.m_r));
    m_data[5] =     (2 *(q.m_j * q.m_k) - 2 *(q.m_i * q.m_r));
    m_data[8] = 1 - (2 *(q.m_i * q.m_i) + 2 *(q.m_j * q.m_j));
}

Matrix3 Matrix3::GetInverse() const
{
    Matrix3 t;
    t.SetInverse(*this);
    return t;
}
        
Matrix3& Matrix3::Invert()
{
    SetInverse(*this);
    return *this;
}

void Matrix3::SetInverse(const Matrix3& m)
{
    float t4 = m.m_data[0]*m.m_data[4];
    float t6 = m.m_data[0]*m.m_data[7];
    float t8 = m.m_data[3]*m.m_data[1];
    float t10 = m.m_data[6]*m.m_data[1];
    float t12 = m.m_data[3]*m.m_data[2];
    float t14 = m.m_data[6]*m.m_data[2];

    // Calculate the determinant
    float t16 = (t4*m.m_data[8] - t6*m.m_data[5] - t8*m.m_data[8]+
                t10*m.m_data[5] + t12*m.m_data[7] - t14*m.m_data[4]);

    // Make sure the determinant is non-zero.
    if (t16 == (float)0.0f) return;
    float t17 = 1/t16;

    m_data[0] = (m.m_data[4]*m.m_data[8]-m.m_data[7]*m.m_data[5])*t17;
    m_data[3] = -(m.m_data[3]*m.m_data[8]-m.m_data[6]*m.m_data[5])*t17;
    m_data[6] = (m.m_data[3]*m.m_data[7]-m.m_data[2]*m.m_data[4])*t17;
    m_data[1] = -(m.m_data[1]*m.m_data[8]-m.m_data[2]*m.m_data[6])*t17;
    m_data[4] = (m.m_data[0]*m.m_data[8]-t14)*t17;
    m_data[7] = -(t6-t10)*t17;
    m_data[2] = (m.m_data[1]*m.m_data[5]-m.m_data[4]*m.m_data[2])*t17;
    m_data[5] = -(m.m_data[0]*m.m_data[5]-t12)*t17;
    m_data[8] = (t4-t8)*t17;
}
/*
* 0 1 2  0 3 6 
* 3 4 5  1 4 7
* 6 7 8  2 5 8
**/
void Matrix3::SetScewSymmetric(const Vec3f& vec)
{
    m_data[0] = m_data[4] = m_data[8] = 0;
    m_data[3] = -vec.GetZ();
    m_data[6] = vec.GetY();
    m_data[1] = vec.GetZ();
    m_data[7] = -vec.GetX();
    m_data[2] = - vec.GetY();
    m_data[5] = vec.GetX();
}

void Matrix3::Print() const
{
}
